clear
%% Data
x = [0.00000000
0.00000000
0.00000000
0.00000000
0.00000000
0.00000000
0.99944794
-0.02966589
0.00004304
0.01495959
0.00000000
0.00000000
0.00000000
-0.00000014
-0.00000002
0.00000000
];

P = [...
0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,9.02199936,0.00000001,0.07924422,0.00893650,0.29855269,0.01787297,-0.00000299,0.00000000,0.00000000,0.00000003,0.00000089,0.00000005
0.00000000,0.00000000,0.00000000,0.00000001,9.02231884,0.12422507,-0.29856327,0.00893683,-0.00000027,0.00000000,-0.00000299,0.00000000,-0.00000089,0.00000003,0.00000000
0.00000000,0.00000000,0.00000000,0.07924423,0.12422507,0.00341568,-0.00403277,0.00274568,0.00015700,0.00000000,0.00000000,-0.00000299,-0.00000001,0.00000001,0.00000000
0.00000000,0.00000000,0.00000000,0.00893650,-0.29856327,-0.00403277,0.01089785,-0.00000001,0.00001771,0.00000000,0.00000000,0.00000000,-0.00000296,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.29855269,0.00893683,0.00274568,-0.00000001,0.01089750,0.00059145,0.00000000,0.00000000,0.00000000,-0.00000000,-0.00000296,0.00000000
0.00000000,0.00000000,0.00000000,0.01787297,-0.00000027,0.00015700,0.00001771,0.00059145,0.00104437,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,-0.00000299
0.00000000,0.00000000,0.00000000,-0.00000299,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000200,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.00000000,-0.00000299,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000200,0.00000000,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,-0.00000299,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000200,0.00000000,0.00000000,0.00000000
0.00000000,0.00000000,0.00000000,0.00000003,-0.00000089,-0.00000001,-0.00000296,-0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000200,0.00000000,-0.00000000
0.00000000,0.00000000,0.00000000,0.00000089,0.00000003,0.00000001,0.00000000,-0.00000296,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000200,-0.00000000
0.00000000,0.00000000,0.00000000,0.00000005,0.00000000,0.00000000,0.00000000,0.00000000,-0.00000299,0.00000000,0.00000000,0.00000000,-0.00000000,-0.00000000,0.00000200];

w_t = [-0.02,0.00,0.01]';

%% Correction

flow = [0.00,0.00]';
f = 65;
p_t = x(1:3);
v_t = x(4:6);
q_t = x(7:10);
wb  = x(14:16);

Sxy = [eye(2,2) zeros(2,1)];
Syx = [0 1 0; -1 0 0];

R_t  = fromqtoR(q_t);
R_ic = diag([1 -1 -1]);
R_wc = R_t*R_ic;

v_cc = R_ic'*R_t'*v_t;
w_cc = R_ic'*(w_t - wb);
z_c  = p_t(3)/R_wc(3,3);

if z_c < 0.1
    z_c = 0.1;
    
    p_t(3) = 0.1;
end

h = -f*Sxy*v_cc/z_c - f*Syx*w_cc;

px = p_t(1);
py = p_t(2);
pz = p_t(3);
vx = v_t(1);
vy = v_t(2);
vz = v_t(3);
qw = q_t(1);
qx = q_t(2);
qy = q_t(3);
qz = q_t(4);

H = [...
    0, 0, -(f*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz))*(qw^2 - qx^2 - qy^2 + qz^2))/pz^2, (f*(qw^2 - qx^2 - qy^2 + qz^2)*(qw^2 + qx^2 - qy^2 - qz^2))/pz,          (f*(2*qw*qz + 2*qx*qy)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, -(f*(2*qw*qy - 2*qx*qz)*(qw^2 - qx^2 - qy^2 + qz^2))/pz,   (2*f*qw*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz + (f*(2*qw*vx - 2*qy*vz + 2*qz*vy)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, (f*(2*qx*vx + 2*qy*vy + 2*qz*vz)*(qw^2 - qx^2 - qy^2 + qz^2))/pz - (2*f*qx*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz, - (2*f*qy*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz - (f*(2*qw*vz - 2*qx*vy + 2*qy*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, (2*f*qz*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz + (f*(2*qw*vy + 2*qx*vz - 2*qz*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz,  0, -f, 0;...
    0, 0,  (f*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz))*(qw^2 - qx^2 - qy^2 + qz^2))/pz^2,         (f*(2*qw*qz - 2*qx*qy)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, -(f*(qw^2 - qx^2 - qy^2 + qz^2)*(qw^2 - qx^2 + qy^2 - qz^2))/pz, -(f*(2*qw*qx + 2*qy*qz)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, - (2*f*qw*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz - (f*(2*qw*vy + 2*qx*vz - 2*qz*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, (2*f*qx*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz - (f*(2*qw*vz - 2*qx*vy + 2*qy*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz,   (2*f*qy*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz - (f*(2*qx*vx + 2*qy*vy + 2*qz*vz)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, (f*(2*qw*vx - 2*qy*vz + 2*qz*vy)*(qw^2 - qx^2 - qy^2 + qz^2))/pz - (2*f*qz*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz, -f,  0, 0];

H_x = [H(:, 1:10), zeros(2, 3), H(:, 11:13)];
X_dx = Qmat(q_t);
X_dx = blkdiag(eye(6), X_dx, eye(6));
H_dx = H_x*X_dx;

Z = H_dx*P*H_dx' + diag([3 3]);
K = P*H_dx'/Z;
S = eye(15) - K*H_dx;
P = S*P*S' + K*diag([3 3])*K';
% P = P - K*Z*K';
% P = (P + P')/2;

z = flow - h;
dx = K*z;
% Reset operation
% x(1:3)   = p_t + dx(1:3);
% x(4:6)   = v_t + dx(4:6);
% qe = vec2q(dx(7:9));
% x(7:10) = qNorm(qProd(q_t,qe));
% x(11:13) = x(11:13) + dx(10:12);
% x(14:16) = wb + dx(13:15);
% dx = zeros(15,1);

%% Symbolic

syms qw qx qy qz
syms f
syms px py pz
syms vx vy vz
syms wx wy wz;
syms wbx wby wbz;

p_t = [px; py; pz];
v_t = [vx; vy; vz];
qv  = [qx; qy; qz];
q_t = [qw; qv];
w_t = [wx; wy; wz];
wb  = [wbx; wby; wbz];

Sxy = [eye(2,2) zeros(2,1)];
Syx = [0 1 0; -1 0 0];

R_t  = fromqtoR(q_t);
R_ic = diag([1 -1 -1]);
R_wc = R_t*R_ic;

v_cc = transpose(R_ic)*transpose(R_t)*v_t;
w_cc = transpose(R_ic)*(w_t-wb);
z_c  = p_t(3)/R_wc(3,3);

h = -f*Sxy*v_cc/z_c - f*Syx*w_cc;

x = [p_t; v_t; q_t; wb];

%H = jacobian(h, x);
H = [...
    0, 0, -(f*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz))*(qw^2 - qx^2 - qy^2 + qz^2))/pz^2, (f*(qw^2 - qx^2 - qy^2 + qz^2)*(qw^2 + qx^2 - qy^2 - qz^2))/pz,          (f*(2*qw*qz + 2*qx*qy)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, -(f*(2*qw*qy - 2*qx*qz)*(qw^2 - qx^2 - qy^2 + qz^2))/pz,   (2*f*qw*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz + (f*(2*qw*vx - 2*qy*vz + 2*qz*vy)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, (f*(2*qx*vx + 2*qy*vy + 2*qz*vz)*(qw^2 - qx^2 - qy^2 + qz^2))/pz - (2*f*qx*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz, - (2*f*qy*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz - (f*(2*qw*vz - 2*qx*vy + 2*qy*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, (2*f*qz*(vx*(qw^2 + qx^2 - qy^2 - qz^2) + vy*(2*qw*qz + 2*qx*qy) - vz*(2*qw*qy - 2*qx*qz)))/pz + (f*(2*qw*vy + 2*qx*vz - 2*qz*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz,  0, -f, 0;...
    0, 0,  (f*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz))*(qw^2 - qx^2 - qy^2 + qz^2))/pz^2,         (f*(2*qw*qz - 2*qx*qy)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, -(f*(qw^2 - qx^2 - qy^2 + qz^2)*(qw^2 - qx^2 + qy^2 - qz^2))/pz, -(f*(2*qw*qx + 2*qy*qz)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, - (2*f*qw*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz - (f*(2*qw*vy + 2*qx*vz - 2*qz*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, (2*f*qx*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz - (f*(2*qw*vz - 2*qx*vy + 2*qy*vx)*(qw^2 - qx^2 - qy^2 + qz^2))/pz,   (2*f*qy*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz - (f*(2*qx*vx + 2*qy*vy + 2*qz*vz)*(qw^2 - qx^2 - qy^2 + qz^2))/pz, (f*(2*qw*vx - 2*qy*vz + 2*qz*vy)*(qw^2 - qx^2 - qy^2 + qz^2))/pz - (2*f*qz*(vy*(qw^2 - qx^2 + qy^2 - qz^2) - vx*(2*qw*qz - 2*qx*qy) + vz*(2*qw*qx + 2*qy*qz)))/pz, -f,  0, 0];

H_x = [H(:, 1:10), zeros(2, 3), H(:, 11:13)];
X_dx = Qmat(q_t);
X_dx = blkdiag(eye(6), X_dx, eye(6));
H_dx = H_x*X_dx;

%% Hackfliht values
H_hack = [...
    0.00000000,0.00000000,0.00000000,19.45002174,-649.13372803,19.32126045,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,-65.00000000,0.00000000,0.00000000;
    0.00000000,0.00000000,0.00000000,-649.42120361,-19.44672966,0.40004042,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,0.00000000,65.00000000,0.00000000];